#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <string>
// #include <geometry_msgs/msg/pose2_d.hpp>
#include "ubt_com/ubt_com_util.h"
#include "ubt_state/ubt_state_types.h"
#include "ubt_state/ubt_state_util.h"
#include "ubt_state/ubt_state_task_nav.h"
#include "ubt_state/ubt_state_task_nav2.h"
#include "ubt_state/ubt_state_task_action.h"
#include "ubt_state/ubt_state_task_adjust.h"
#include "ubt_state/ubt_state_task_charge_ex.h"
#include "ubt_state/ubt_state_task_ex.h"
#include "ubt_state/ubt_state_api.h"
#include "ubt_state/node.h"

namespace ubt_state
{

struct TaskExClsData
{
    std::string _charge_type;
    float _presision_adjust;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr _sub_ui_state; 
    CTaskEx *_pCurTask;
};

static TaskExClsData g_clsData;

void ui_state_CallbackWrap(const std_msgs::msg::String::SharedPtr msg)
{
    if (g_clsData._pCurTask != NULL)
    {
        g_clsData._pCurTask->ui_state_Callback(msg);
    }
}
int CTaskEx::ClassInit(ObstacleParams obsParams)
{
    CTaskNav::ClassInit(obsParams);
    CTaskNav2::ClassInit(obsParams);
    CTaskAction::ClassInit();
    CTaskAdjust::ClassInit();
    CTaskCharge::ClassInit();
    TaskExClsData *pClsData = &g_clsData;
    pClsData->_sub_ui_state = node->create_subscription<std_msgs::msg::String>("/ui_state", 10, std::bind(&ui_state_CallbackWrap, std::placeholders::_1));  
    node->declare_parameter<std::string>("/charge_type", std::string("uslam"));
    pClsData->_charge_type = node->get_parameter("/charge_type").as_string();
    // node->declare_parameter<float>("/presision_adjust", 0.0);
    // pClsData->_presision_adjust = node->get_parameter("/presision_adjust").as_float();
}
void CTaskEx::ClassCleanup()
{
    CTaskCharge::ClassCleanup();
    CTaskAdjust::ClassCleanup();
    CTaskAction::ClassCleanup();
    CTaskNav2::ClassCleanup();
    CTaskNav::ClassCleanup();
}

ITask *CTaskEx::CreateTask(ITaskCb *pTaskCb, std::string taskId, std::string taskInfo, int navType)
{
    CTaskEx *pTask = new CTaskEx();
    if (!pTask->Init(pTaskCb, taskId, taskInfo, navType))
    {
        delete pTask;
        pTask = NULL;
    }
    return (ITask *)pTask;
}

CTaskEx::~CTaskEx()
{
    std::vector<ITask *>::iterator iter;
    for (iter = _pTaskArr.begin(); iter != _pTaskArr.end(); ++iter)
    {
        delete *iter;
    }
    _pTaskArr.clear();
    _curTask = NULL;
    _curTaskIndex = -1;
}
void CTaskEx::ui_state_Callback(const std_msgs::msg::String::SharedPtr msg)
{
    
    
    if(msg->data == "mission_action_wait_cancel" && _curTask != NULL && _curTask->GetActionType() =="online-charge")//取消在线充电任务
    {
        _curTask->Stop(ubt_task_result_ok);
        _state =  ubt_task_state_running;
        _StartNextTask();
    }
    
}

bool CTaskEx::Init(ITaskCb *pTaskCb, std::string taskId, std::string taskInfo, int navType)
{
    std::string agv_type;
    node->declare_parameter("agv_type", "");
     agv_type = node->get_parameter("agv_type").as_string();

    int iRet, taskIndex, reportIndex;
    bool bRet = true;
    std::string failMsg, failReason;
    ITask *pTask;
    _pTaskCb = pTaskCb;
    _state = ubt_task_state_idle;
    _stopReason = ubt_task_result_ok;
    _curTask = NULL;
    _curTaskIndex = -1;
    g_clsData._pCurTask = this;

    taskIndex = 0;
    reportIndex = taskIndex;
    
    do
    {
	    RCLCPP_INFO(node->get_logger(),"taskInfo:::::%s",taskInfo.c_str());
        iRet = ParseFixedPathTaskInfo(taskInfo, &_taskInfo, failReason);
        if (iRet != 0)
        {
            char buf[64];
            sprintf(buf, "parse taskinfo fail= %d, failReason= ", iRet);
            failMsg = std::string(buf) + failReason;
            bRet = false;
            break;
        }
        StateStatus *pStateStatus = _pTaskCb->GetStateStatus();
        // 导航
        if (_taskInfo.path.size() > 0)
        {
            ubt_task_stage stage = ubt_task_stage_run;
            if (_taskInfo.props == UBT_TASK_PROP_CHARGE_POINT)
            {
                stage = ubt_task_stage_recharge;
                _taskInfo.path[_taskInfo.path.size() - 1].points[1].avoidDis = 0;
            }
            else if (_taskInfo.props != UBT_TASK_PROP_STATION_POINT)
            {
                _taskInfo.path[_taskInfo.path.size() - 1].points[1].avoidDis = 0.02;
            }
            else
            {
            }
            int prePathAvoidHwMask = 0;
#ifdef UBT_LIFT_SUPPORT
            if (pStateStatus->isUnderShelve)
            {
                if (pStateStatus->liftState != lift_state_up)
                {
                    double xOff = pStateStatus->shelvePose.x - pStateStatus->curPose.x;
                    double yOff = pStateStatus->shelvePose.y - pStateStatus->curPose.y;
                    double dis2 = xOff * xOff + yOff * yOff;
                    if (dis2 < 4)
                    {
                        prePathAvoidHwMask = avoid_hw_mask_rgbd;
                    }
                }
                if (prePathAvoidHwMask == 0)
                {
                    pStateStatus->isUnderShelve = false;
                }
            }
#endif
            taskIndex++;
            reportIndex++;
            if (navType == 0)
            {
                pTask = CTaskNav::CreateTask(this, _taskInfo.taskId, &_taskInfo.path, stage, 0, taskIndex, reportIndex);
            }
            else
            {
                pTask = CTaskNav2::CreateTask(this, _taskInfo.taskId, &_taskInfo.path, stage, 0, taskIndex, reportIndex);
            }
            if (pTask == NULL)
            {
                failMsg = "create nav task fail";
                bRet = false;
                break;
            }
#ifdef AVOID_CAR_SUPPORT
            if (_taskInfo.props == UBT_TASK_PROP_WAIT_POINT)
            {
                ((CTaskNav *)pTask)->SetCarParams(1.1, _taskInfo.waitTime);
            }
#endif
            _pTaskArr.push_back(pTask);
        }
        if (_taskInfo.props == UBT_TASK_PROP_CHARGE_POINT) //固定路径，充电点
        {
		            //判断是否有online_charge,有则跳过
            bool have_online_charge = false;
            for (int i = 0; i < _taskInfo.actions.size(); i++)
            {
                Action action = _taskInfo.actions[i];
                if(action.type == UBT_ACTION_ONLINE_CHARGE)
                {
                    have_online_charge = true;
                }
            }
            ////////
            if(!have_online_charge)//没有online_charge才能创建这个任务
            {
                if (_taskInfo.path.size() == 0)
                {
                    failMsg = "no line";
                    bRet = false;
                    break;
                }
                taskIndex++;
                reportIndex++;
                pTask = CTaskChargeEx::CreateTask(this, &_taskInfo, taskIndex, reportIndex, -1, g_clsData._charge_type);
                _pTaskArr.push_back(pTask);
                break;
            }
        }
        int liftState = lift_state_up;
        int dragState = drag_state_up;
        int forkState = fork_state_unknown;

        forkState = pStateStatus->forkState;
#ifdef UBT_LIFT_SUPPORT
        liftState = pStateStatus->liftState;
#endif
#ifdef UBT_DRAG_SUPPORT
        dragState = pStateStatus->dragState;
#endif
        int avoidHwMask;
        for (int i = 0; i < _taskInfo.actions.size(); i++)
        {
            avoidHwMask = 0;
            Action action = _taskInfo.actions[i];
            taskIndex++;
            reportIndex++;

            if (action.type == UBT_ACTION_ADJUST_CODE)
            {
                if ((_taskInfo.props == UBT_TASK_PROP_SHELF_POINT) && (liftState != lift_state_up))
                {
                    avoidHwMask |= avoid_hw_mask_rgbd;
                }
                geometry_msgs::msg::Pose2D poseDst, poseLmt = PoseZero;
                poseDst.x = poseDst.y = 0;
                poseDst.theta = ubt_proto_adjust::ubt_adjust_theta_4x90;
                pTask = CTaskAdjust::CreateTask(this, _taskInfo.taskId, ubt_task_stage_action, 120000,
                                                ubt_proto_adjust::ubt_adjust_pose_qrcode, poseDst, PoseZero,
                                                0.01, 0.05, avoidHwMask,
                                                taskIndex, reportIndex, UBT_ACTION_ADJUST_CODE);
            }

            else if (action.type == UBT_ACTION_MOVE_FORWARD)
            {
                if ((_taskInfo.props == UBT_TASK_PROP_SHELF_POINT) && (liftState != lift_state_up))
                {
                    avoidHwMask |= avoid_hw_mask_rgbd;
                }
                
                geometry_msgs::msg::Pose2D poseDst;

                int n;
                float f0, f1, f2;
                
                n = sscanf(action.value.c_str(), "%f|%f|%f", &f0, &f1, &f2);
                std::string presision_adjust_str = "";
                node->declare_parameter("/presision_adjust", "0.0");
                 presision_adjust_str = node->get_parameter("/presision_adjust").as_string();
                float presision_adjust = 0.0;
                try
                {
                    presision_adjust = (std::stof(presision_adjust_str) / 1000);
                }
                catch(const std::exception& e)
                {
                    RCLCPP_WARN(node->get_logger(),"CTaskEx::Init param of presision_adjust failed.");
                    std::cerr << "Invalid argument:" << e.what() << '\n';
                    presision_adjust = 0.0;
                }
                
                f0 -= presision_adjust;
                if(n == 3)
                {
                    poseDst.x = f0;
                    poseDst.y = 0;
                    poseDst.theta = 0;
                }
                else if(n == 1)
                {
                    f1 = 0.0;
                    f2 = 0.0;
                    poseDst.x = f0;
                    poseDst.y = 0;
                    poseDst.theta = 0;
                }
                else
                {
                    f0 = f1 = f2 = 0.0;
                    poseDst.x = 0.0;
                    poseDst.y = 0.0;
                    poseDst.theta = 0.0;
                }

                RCLCPP_INFO(node->get_logger(),"n=%d, f0=%f, f1=%f, f2=%f", n, f0, f1, f2);

                /* 允许误差 */
                geometry_msgs::msg::Pose2D poseLmt;
                poseLmt.x = 0.03;
                poseLmt.y = 0.03;
                poseLmt.theta = 0.01;
                pTask = CTaskAdjust::CreateTask(this, _taskInfo.taskId, ubt_task_stage_action, 120000,
                                                ubt_proto_adjust::ubt_adjust_pose_odomrel, poseDst, poseLmt,
                                                f1, f2, avoidHwMask,
                                                taskIndex, reportIndex, UBT_ACTION_MOVE_FORWARD);
            }

            else if (action.type == UBT_ACTION_ONLINE_CHARGE)
            { 
                //通知充电模块
                Line lastLine = _taskInfo.path[_taskInfo.path.size() - 1];
                Point lastPoint = lastLine.points[1];
                geometry_msgs::msg::Pose2D  poseDst;
                poseDst.x = lastPoint.x;
                poseDst.y = lastPoint.y;
                poseDst.theta = lastPoint.t;
                pTask = CTaskChargeEx::CreateTask(this, &_taskInfo, taskIndex, reportIndex, atoi(action.value.c_str()), g_clsData._charge_type);
            }

            else if (action.type == UBT_ACTION_ROTATE)
            {
                if ((_taskInfo.props == UBT_TASK_PROP_SHELF_POINT) && (liftState != lift_state_up))
                {
                    avoidHwMask |= avoid_hw_mask_rgbd;
                }
                
                geometry_msgs::msg::Pose2D poseDst;
                poseDst.x = 0;
                poseDst.y = 0;
                poseDst.theta = atof(action.value.c_str())/-180.0*M_PI;
                
                /* 允许误差 */
                geometry_msgs::msg::Pose2D poseLmt;
                poseLmt.x = 0.03;
                poseLmt.y = 0.03;
                poseLmt.theta = 0.01;
                
                pTask = CTaskAdjust::CreateTask(this, _taskInfo.taskId, ubt_task_stage_action, 120000,
                                                ubt_proto_adjust::ubt_adjust_pose_odomrel, poseDst, poseLmt,
                                                0.0, 0.0, avoidHwMask,
                                                taskIndex, reportIndex, UBT_ACTION_MOVE_FORWARD);
            }

            else  // action
            {
                //顶升时
                if (action.type == UBT_ACTION_RISE)
                {
                    if (liftState == lift_state_up)
                    {
                        char buf[128];
                        snprintf(buf, sizeof(buf) - 1, "action rise need liftState=%d,but liftState=%d ", lift_state_down, liftState);
                        buf[sizeof(buf) - 1] = 0;
                        failMsg = buf;
                        bRet = false;
                        break;
                    }
                    //自动在开始处添加下降操作
                    if (liftState != lift_state_down)
                    {
                        Action action;
                        action.type = UBT_ACTION_FALL;
                        action.value = "";
                        pTask = CTaskAction::CreateTask(this, _taskInfo.taskId, &action, taskIndex, reportIndex);
                        _pTaskArr.insert(_pTaskArr.end(), pTask);
                        liftState = lift_state_down;
                        taskIndex++;
                    }
                    //自动添加精定位操作, 关闭避障
                    geometry_msgs::msg::Pose2D poseDst;
                    poseDst.x = poseDst.y = 0;
                    poseDst.theta = ubt_proto_adjust::ubt_adjust_theta_4x90;
                    pTask = CTaskAdjust::CreateTask(this, _taskInfo.taskId, ubt_task_stage_action, 120000,
                                                    ubt_proto_adjust::ubt_adjust_pose_qrcode1, poseDst, PoseZero,
                                                    0.0, 0.0, avoid_hw_mask_rgbd,
                                                    taskIndex, reportIndex, "rise-adjust");

                    _pTaskArr.push_back(pTask);
                    liftState = lift_state_up;
                    taskIndex++;
                }
                else if (action.type == UBT_ACTION_FALL)
                {
                    if (liftState == lift_state_down)
                    {
                        char buf[128];
                        snprintf(buf, sizeof(buf) - 1, "action fall need liftState!=down");
                        buf[sizeof(buf) - 1] = 0;
                        failMsg = buf;
                        bRet = false;
                        break;
                    }
                    liftState = lift_state_down;
                }
                else if (action.type == UBT_ACTION_GRAG_LIFT)
                {
                   if (dragState == drag_state_down && action.value == "0")
                    {
                        char buf[128];
                        snprintf(buf, sizeof(buf) - 1, "action already down");
                        buf[sizeof(buf) - 1] = 0;
                        failMsg = buf;
                        bRet = false;
                        break;
                    }
                    else if (dragState == drag_state_up && action.value == "1")
                    {
                        char buf[128];
                        snprintf(buf, sizeof(buf) - 1, "action already up");
                        buf[sizeof(buf) - 1] = 0;
                        failMsg = buf;
                        bRet = false;
                        break;
                    }
                    if(action.value == "0")
                    {
                        dragState = drag_state_down;
                    }else if(action.value == "1")
                    {
                        dragState = drag_state_up;
                    }
                }


                else if (action.type == UBT_ACTION_FORK_MEASURE)
                {

                }

                else if (action.type == UBT_ACTION_FORK_MEASURE_HEIGHT)
                {

                }

                else if (action.type == UBT_ACTION_FROK_TO_STATION)
                {

                }

                pTask = CTaskAction::CreateTask(this, _taskInfo.taskId, &action, taskIndex, reportIndex);
                }

            if (pTask == NULL)
            {
                failMsg = "create action task fail";
                bRet = false;
                break;
            }
            _pTaskArr.push_back(pTask);
        }
    }
    while (false);

	if(!bRet)
	{
    	RCLCPP_INFO(node->get_logger(),"taskId=%s bRet=%d failMsg=%s taskInfo=%s",
        	taskId.c_str(), bRet, failMsg.c_str(), taskInfo.c_str());
	}
    RCLCPP_INFO(node->get_logger(),"_pTaskArr task");
    for(int i = 0;i<_pTaskArr.size();i++)
    {
        RCLCPP_INFO(node->get_logger(),"_pTaskArr.taskType=%s",_pTaskArr[i]->GetActionType());
    }
    
	return bRet;
}

StateStatus *CTaskEx::GetStateStatus()
{
    return _pTaskCb->GetStateStatus();
}

void CTaskEx::NotifyTaskEvent(std::string taskEvent)
{
    _pTaskCb->NotifyTaskEvent(taskEvent);
}

void CTaskEx::NotifyTaskStateAndStage(int state, int stage, int stateReason, std::string stateReasonDesc)
{
    RCLCPP_INFO(node->get_logger(),"[CTaskEx] NotifyTaskStateAndStage ==================== start ====================");
    RCLCPP_INFO(node->get_logger(),"[CTaskEx] NotifyTaskStateAndStage _curTaskIndex=%d  state=%d stage=%d stateReason=%d stateReasonDesc=%s",
             _curTaskIndex, state, stage, stateReason, stateReasonDesc.c_str());

    ubt_state::ulogger->info("[CTaskEx] NotifyTaskStateAndStage : _pTaskArr.size={}, _curTaskIndex={}, state={}, stage={}, stateReason={}, stateReasonDesc={}",
             _pTaskArr.size(), _curTaskIndex, state, stage, stateReason, stateReasonDesc.c_str());

    do
    {
        if(_curTask->GetStage() == ubt_task_stage_recharge_fail)//可恢复故障，不再执行任务
        {
            Stop(ubt_task_result_ok);
            _pTaskCb->NotifyTaskStateAndStage(state, stage, stateReason, stateReasonDesc);
            break;
        }
        if (state != ubt_task_state_closed)
        {
            _pTaskCb->NotifyTaskStateAndStage(state, stage, stateReason, stateReasonDesc);
            break;
        }
        if (stateReason != 0)
        {
            // std::string taskEvent = GetTaskEventByReason(stateReason);
            // _pTaskCb->NotifyTaskEvent(taskEvent);
            _pTaskCb->NotifyTaskStateAndStage(state, stage, stateReason, stateReasonDesc);
            break;
        }
        if (_curTaskIndex >= (_pTaskArr.size() - 1))
        {
            // _pTaskCb->NotifyTaskEvent(UBT_TASK_EVENT_FINISH_STR);
            _pTaskCb->NotifyTaskStateAndStage(state, stage, 0, "");
            break;
        }
        _StartNextTask();
    }
    while (false);
    
}

bool CTaskEx::Start()
{
    RCLCPP_INFO(node->get_logger(),"_state=%d", _state);
    if (_state != ubt_task_state_idle)
    {
        return false;
    }
    _state = ubt_task_state_running;
    _pTaskCb->NotifyTaskEvent(UBT_TASK_EVENT_PROCESS_STR);
    _StartNextTask();
    return true;
}

void CTaskEx::Pause()
{
    RCLCPP_INFO(node->get_logger(),"_state=%d", _state);
    if (_state == ubt_task_state_running)
    {
        _state = ubt_task_state_pause;
        _curTask->Pause();
    }
}

void CTaskEx::Continue()
{
    RCLCPP_INFO(node->get_logger(),"_state=%d", _state);
    if (_state == ubt_task_state_pause)
    {
        _state = ubt_task_state_running;
        if (_curTask->GetState() == ubt_task_state_idle)
        {
            _curTask->Start();
        }
        else
        {
            _curTask->Continue();
        }
    }
}

void CTaskEx::Stop(int reason)
{
    RCLCPP_INFO(node->get_logger(),"CTaskEx::Stop _state=%d reason=%d", _state, reason);
    if (_state >= ubt_task_state_stopping)
    {
        if(reason == ubt_task_result_usr_stopbtn)//按下急停传递给charge_ex处理
        {
            _curTask->Stop(reason);
        }
        return;
    }
    _state = ubt_task_state_stopping;
    _stopReason = reason;
    _curTask->Stop(reason);
}

void CTaskEx::OnTimer()
{
    _curTask->OnTimer();
}

void CTaskEx::OnMsg(int msgType, void *pvMsg)
{
    _curTask->OnMsg(msgType, pvMsg);
}

ubt_task_type CTaskEx::GetType()
{
    return ubt_task_type_ex;
}

ubt_task_state CTaskEx::GetState()
{
    ubt_task_state taskState = _curTask->GetState();
    if ((taskState == ubt_task_state_idle) && (_state == ubt_task_state_pause))
    {
        taskState = ubt_task_state_pause;
    }
    return taskState;
}

ubt_task_stage CTaskEx::GetStage()
{
    return _curTask->GetStage();
}

std::string CTaskEx::GetActionType()
{
    return _curTask->GetActionType();
}
void CTaskEx::GetIndex(int& index, int& reportIndex)
{
    _curTask->GetIndex(index, reportIndex);
}

void CTaskEx::GetStopInfo(int& stopReason, std::string& stopReasonDesc)
{
    _curTask->GetStopInfo(stopReason, stopReasonDesc);
}
void CTaskEx::GetLogs(std::vector<TaskLogItem>& logs)
{
    _curTask->GetLogs(logs);
}

void CTaskEx::_StartNextTask()
{
    
    ubt_state::ulogger->info("[CTaskEx] _StartNextTask : _pTaskArr.size={}, _curTaskIndex={}, _state={}",
             _pTaskArr.size(), _curTaskIndex, _state);
    
    _curTaskIndex++;
    RCLCPP_INFO(node->get_logger(),"_curTaskIndex:%d    _pTaskArr.size():%d",_curTaskIndex,_pTaskArr.size());
    
    if(_curTaskIndex >= _pTaskArr.size())//数组越界判断
    {
        return;
    }
    _curTask = _pTaskArr[_curTaskIndex];
    if (_state == ubt_task_state_stopping)
    {
        _pTaskCb->NotifyTaskStateAndStage(ubt_task_state_closed, _curTask->GetStage(), _stopReason, "");
        return;
    }
    if (_state == ubt_task_state_running)
    {
        _curTask->Start();
    }
    _pTaskCb->NotifyTaskStateAndStage(_curTask->GetState(), _curTask->GetStage(), 0, "");
    
}

}


